#include "vehicle_linear2d2s.h"
#include <cmath>
////init state fun when speed is 1
using namespace LinearMpc;
namespace auto_ros
{
namespace control
{
VehicleLinear2d2s::VehicleLinear2d2s(double mass, double cf, double cr, double lf, double lr, double iz,
									 double max_lateral_acc, double max_tire_f_angle)
	: ContinuousLinearSys(4, 1, 1), mass_(mass), cf_(cf), cr_(cr), lf_(lf), lr_(lr), iz_(iz), max_lateral_acc_(max_lateral_acc),
	  max_tire_f_angle_(max_tire_f_angle), base_state_matrix_(4, 4), base_distur_matrix_(4, 1) //init state fun when speed is 1m/s
{
	speed_threshold_ = 0.05;
	state_matrix_ << 0, 1, 0, 0,
		0, -2 * (cf_ + cr_) / mass_, 2 * (cf_ + cr_) / mass_, 2 * (-cf_ * lf_ + cr_ * lr_) / mass_,
		0, 0, 0, 1,
		0, 2 * (-cf_ * lf_ + cr_ * lr_) / iz_, 2 * (cf_ * lf_ - cr_ * lr_) / iz_, -2 * (cf_ * lf_ * lf_ + cr_ * lr_ * lr_) / iz_;
	base_state_matrix_ = state_matrix_;
	control_matrix_ << 0, 2 * cf_ / mass_, 0, 2 * cf_ * lf_ / iz_;
	distur_matrix_ << 0, 2 * (-cf_ * lf_ + cr_ * lr_) / mass_ - 1, 0, -2 * (cf_ * lf_ * lf_ + cr_ * lr_ * lr_) / iz_;
	base_distur_matrix_ = distur_matrix_;
}
void VehicleLinear2d2s::speed_update_matrix(double speed)
{
	if (std::abs(speed) < speed_threshold_)
	{
		if (speed >= 0)
			speed = speed_threshold_;
		else
			speed = -speed_threshold_;
	}

	state_matrix_(1, 1) = base_state_matrix_(1, 1) / speed;
	state_matrix_(1, 3) = base_state_matrix_(1, 3) / speed;
	state_matrix_(3, 1) = base_state_matrix_(3, 1) / speed;
	state_matrix_(3, 3) = base_state_matrix_(3, 3) / speed;
	distur_matrix_(1, 0) = state_matrix_(1, 3) - speed;
	distur_matrix_(3, 0) = state_matrix_(3, 3);
}
bool VehicleLinear2d2s::set_mass(double mass)
{
	if (mass <= 0)
	{
		std::cout << "\033[31m Error VehicleLinear2d2s::set_mass failed,mass <= 0 \033[0m" << std::endl;
		return false;
	}
	mass_ = mass;
	return true;
}
bool VehicleLinear2d2s::set_lf(double lf)
{
	if (lf <= 0)
	{
		std::cout << "\033[31m Error VehicleLinear2d2s::set_lf failed,lf <= 0 \033[0m" << std::endl;
		return false;
	}
	lf_ = lf;
	return true;
}
bool VehicleLinear2d2s::set_lr(double lr)
{
	if (lr <= 0)
	{
		std::cout << "\033[31m Error VehicleLinear2d2s::set_lr failed,lr <= 0 \033[0m" << std::endl;
		return false;
	}
	lr_ = lr;
	return true;
}
bool VehicleLinear2d2s::set_cf(double cf)
{
	if (cf <= 0)
	{
		std::cout << "\033[31m Error VehicleLinear2d2s::set_cf failed,cf <= 0 \033[0m" << std::endl;
		return false;
	}
	cf_ = cf;
	return true;
}
bool VehicleLinear2d2s::set_cr(double cr)
{
	if (cr <= 0)
	{
		std::cout << "\033[31m Error VehicleLinear2d2s::set_cr failed,cr <= 0 \033[0m" << std::endl;
		return false;
	}
	cr_ = cr;
	return true;
}
bool VehicleLinear2d2s::set_iz(double iz)
{
	if (iz <= 0)
	{
		std::cout << "\033[31m Error VehicleLinear2d2s::set_iz failed,iz <= 0 \033[0m" << std::endl;
		return false;
	}
	iz_ = iz;
	return true;
}
bool VehicleLinear2d2s::set_max_lateral_acc(double max_acc)
{
	if (max_acc <= 0)
	{
		std::cout << "\033[31m Error VehicleLinear2d2s::set_max_lateral_acc failed,set_max_lateral_acc <= 0 \033[0m" << std::endl;
		return false;
	}
	max_lateral_acc_ = max_acc;
	return true;
}
bool VehicleLinear2d2s::set_max_tire_f_angle(double max_angle)
{
	if (max_angle <= 0 || max_angle > 1.0)
	{
		std::cout << "\033[31m Error VehicleLinear2d2s::set_max_tire_f_angle failed,max_angle <= 0 || max_angle > 1.0 \033[0m" << std::endl;
		return false;
	}
	max_tire_f_angle_ = max_angle;
	return true;
}
double VehicleLinear2d2s::mass()
{
	return mass_;
}
double VehicleLinear2d2s::lf()
{
	return lf_;
}
double VehicleLinear2d2s::lr()
{
	return lr_;
}
double VehicleLinear2d2s::cf()
{
	return cf_;
}
double VehicleLinear2d2s::cr()
{
	return cr_;
}
double VehicleLinear2d2s::iz()
{
	return iz_;
}

double VehicleLinear2d2s::max_tire_f_angle()
{
	return max_tire_f_angle_;
}
void VehicleLinear2d2s::update_base_matrix()
{
	state_matrix_ << 0, 1, 0, 0,
		0, -2 * (cf_ + cr_) / mass_, 2 * (cf_ + cr_) / mass_, 2 * (-cf_ * lf_ + cr_ * lr_) / mass_,
		0, 0, 0, 1,
		0, 2 * (-cf_ * lf_ + cr_ * lr_) / iz_, 2 * (cf_ * lf_ - cr_ * lr_) / iz_, -2 * (cf_ * lf_ * lf_ + cr_ * lr_ * lr_) / iz_;
	base_state_matrix_ = state_matrix_;
	control_matrix_ << 0, 2 * cf_ / mass_, 0, 2 * cf_ * lf_ / iz_;
	distur_matrix_ << 0, 2 * (-cf_ * lf_ + cr_ * lr_) / mass_ - 1, 0, -2 * (cf_ * lf_ * lf_ + cr_ * lr_ * lr_) / iz_;
	base_distur_matrix_ = distur_matrix_;
}
bool VehicleLinear2d2s::set_parameter(double mass, double lf, double lr,
									  double cf, double cr, double iz, double max_acc, double max_angle)
{
	if (!set_mass(mass))
		return false;
	if (!set_lf(lf))
		return false;
	if (!set_lf(lr))
		return false;
	if (!set_cf(cf))
		return false;
	if (!set_cr(cr))
		return false;
	if (!set_iz(iz))
		return false;
	if (!set_max_lateral_acc(max_acc))
		return false;
	if (!set_max_tire_f_angle(max_angle))
		return false;
	update_base_matrix();
	return true;
}
double VehicleLinear2d2s::speed_update_max_tire_f_angle(double speed)
{
	if (std::abs(speed) < speed_threshold_)
	{
		if (speed > 0)
			speed = speed_threshold_;
		else
			speed = -speed_threshold_;
	}
	double max_angle = max_lateral_acc_ * (lf_ + lr_) / (speed * speed);
	max_angle = std::min(max_angle, max_tire_f_angle_);
	return max_angle;
}
double VehicleLinear2d2s::steady_heading_error(double speed, double kappa)
{
	return -1 * lr_ * kappa + lf_ / 2 / cr_ / (lf_ + lr_) * (mass_ * speed * speed * kappa);
}
double VehicleLinear2d2s::steady_steering_angle(double speed, double kappa)
{
	double k_v = lr_ * mass_ / 2 / cf_ / (lf_ + lr_) - lf_ * mass_ / 2 / cr_ / (lf_ + lr_);
	return (lf_ + lr_) * kappa + k_v * speed * speed * kappa;
}
Eigen::MatrixXd VehicleLinear2d2s::base_state_matrix()
{
	return base_state_matrix_;
}
} // namespace control
} // namespace auto_ros
